import threading
import time
import numpy as np
from threading import Thread
from UDPCommunicator import UDPCommunicator
from ClassicWashOut import ClassicWashOut
from StewartPlatform import StewartPlatform
from draw_3d_spiral import draw_3d_spiral
from generate_elliptical_points import generate_elliptical_points

# 构造六自由度运动平台
path = "Stewart/Stewart.urdf"
joint_indices = [(6, 16), (35, 17), (49, 18), (42, 19), (28, 20)]
actuator_indices = [9, 2, 31, 45, 38, 24]
radious_platform, radious_base = 0.2, 0.2         # meters
half_angle_platform, half_angle_base = 24/2, 24/2 # degrees
design_variables = [radious_platform, radious_base, half_angle_platform, half_angle_base]

# 构建经典洗出算法滤波器
washout = ClassicWashOut()
washout.load()

platform = StewartPlatform(path, joint_indices, actuator_indices, design_variables)
platform.set_env()
platform.set_constraints()
platform.init_stewart(True)

initTrans = np.array([0, 0, 0])  # 9 cm in z axis 回到初始零位
initRots = np.array([0, 0, 0])  # Roll, pitch and yaw angles of the platform
initTime = 3
initData = [[initTrans, initRots, 3]]
platform.start_simmulation(initData, simulation=False, flag=False)

movedata = [[]] #实时传递给平台工作线程的参数
lock = threading.Lock()
data_ready = threading.Event()

def platformtask():
    while True:
        data_ready.wait()
        # start_time = time.time()
        # end_time = time.time()
        # print("耗时: {:.2f}秒".format(end_time - start_time))

        platform.start_simmulation(movedata, simulation=False, flag=False)  # 执行时间1s左右

        # with lock:
        #     if not movedata:
        #         data_ready.clear()
        #         continue
        #     platform.start_simmulation(movedata, simulation=False, flag=False)

if __name__ == '__main__':
    # 接收来自手机的加速度计与陀螺仪数据 手机APP HyperIMU 数据频率默认为100ms 修改为50ms与洗出算法积分步长保持一致
    # 后续可以根据接收或者读取记录的加速度数据值
    server = UDPCommunicator(host='192.168.124.2', port=2055, timeout=1)
    server.start()
    print("UDP服务端已启动，等待传感器数据...")

    platformThread = threading.Thread(target=platformtask)
    platformThread.start()

    while True:
        if server.recvdata:
            # print(f"收到来自 {addr} 的消息: {data.decode()}")
            sensordata = server.recvdata.decode()
            sensordata = sensordata.replace('\r', '').replace('\n', '')
            sensordata_list = sensordata.split(",")

            # 加速度计数据 单位 m/s2 坐标系定义与极性：垂直手机屏幕尾Z 向右为X  向前为Y
            AccelData_str = [sensordata_list[0], sensordata_list[0], sensordata_list[2]]
            AccelData = [float(sensordata_list[0]), float(sensordata_list[1]), float(sensordata_list[2])]

            # 陀螺仪数据 单位 rad/s 坐标系定义与极性：垂直手机屏幕尾Z 向右为X  向前为Y
            AngularData_str = [sensordata_list[3], sensordata_list[4], sensordata_list[5]]
            AngularData = [float(sensordata_list[3]), float(sensordata_list[4]), float(sensordata_list[5])]

            dataToWashout = [AccelData[0], AccelData[1], AccelData[2], AngularData[0], AngularData[1], AngularData[2]]

            # 经典洗出算法计算出运动平台所需的位移量与姿态
            washoutResult = washout.handle_input_signal(dataToWashout)

            # 进行Stewart机构运动学逆解 计算出各支柱伸长量 并绘制出运动平台的实时位置与姿态
            trans = washoutResult[0]
            rots = washoutResult[1]
            moveStepTime = 0.4 # 与pwm占空比周期有关 占空比周期20ms 50%占空比

            movX = float(trans[0])
            movY = float(trans[1])
            movZ = float(trans[2])
            rotsX = float(rots[0])
            rotsY = float(rots[1])
            rotsZ = float(rots[2])

            # transVec = np.array([movX, movY, movZ])
            # rotsVec = np.array([rotsX, rotsY, rotsZ])

            # 测试
            transVec = np.array([movX, movY, movZ]) * 0.25
            rotsVec = np.array([rotsX, rotsY, rotsZ]) * 0.25

            # 有锁的情况需要等待工作线程解锁之后 数据才会更新 平台线程处理很耗时
            # with lock:
            #     movedata = [[transVec, rotsVec, moveStepTime]]
            #     print(movedata)
            # data_ready.set()

            movedata = [[transVec, rotsVec, moveStepTime]]
            data_ready.set()










